/*!
  @file preprocessor.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include "axis_netcam/preprocessor.h"
#include <opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>

namespace axis_netcam
{

void Preprocessor::imageCB(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
{
  cv_bridge::CvImagePtr cv_ptr;
  try
  {
    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
  }
  catch (cv_bridge::Exception &e)
  {
    ROS_ERROR("Proprocessor: %s", e.what());
    return;
  }
  image_ = cv_ptr->image;

  /* receive K and D */
  K_.create(3, 3, CV_32F);
  D_.create(1, 5, CV_32F);
  int j;
  for(j=0;j<3;j++) K_.cv::Mat::col(j).cv::Mat::row(0)=info_msg->K[j];
  for(j=0;j<3;j++) K_.cv::Mat::col(j).cv::Mat::row(1)=info_msg->K[j+3];
  for(j=0;j<3;j++) K_.cv::Mat::col(j).cv::Mat::row(2)=info_msg->K[j+6];
  for(j=0;j<5;j++) D_.cv::Mat::col(j)=info_msg->D[j]; 


  /* do some processing */
  undistort();
  // rectify();

  /* republish image */
  cv_ptr->image = undistortimage_;
  img_pub_.publish(cv_ptr->toImageMsg(), info_msg);
}

void Preprocessor::undistort()
{
  if (!initCompute())
  {
    ROS_ERROR("parameter not initialized ??");
    return;
  }

  cv::undistort(image_,undistortimage_,K_,D_);
}

void Preprocessor::rectify()
{
  if (!initCompute())
  {
    ROS_ERROR("parameter not initialized ??");
    return;
  }

  /* TODO write process here */
  

}

}  // namespace myns

int main(int ac, char **av)
{
  ros::init(ac, av, "Preprocessor");
  ros::NodeHandle nh;
  axis_netcam::Preprocessor handler(nh);
  ros::spin();
  return 0;
}

